
#include <talos/arduino/ArduinoFirmata.hpp>

#define MAX( a, b ) ((a) < (b)) ? (b) : (a)

namespace talos
{

	ArduinoFirmata::ArduinoFirmata( const std::string serialPort, const int baudRate ) :
		Arduino( serialPort, baudRate ) {}

	void ArduinoFirmata::queryFirmware()
	{
		writeByte( START_SYSEX );
		writeByte( FIRMWARE_NAME_VS );
		writeByte( END_SYSEX );
	}

	void ArduinoFirmata::setPWM( uint8_t pinNumber, uint16_t value )
	{
		value16_t value_; value_.int16_ = value;
		writeByte( ANALOG_MESSAGE + pinNumber );
		writeByte( value_.int8_[0] );
		writeByte( value_.int8_[1] );
	}

	void ArduinoFirmata::setAnalog( uint8_t pinNumber, uint16_t state )
	{
		value16_t state_; state_.int16_ = state;
		writeByte( REPORT_ANALOG + pinNumber );
		writeByte( state_.int8_[0] );
		writeByte( state_.int8_[1] );
	}

	/*
	   void ArduinoFirmata::configureServo( uint8_t pinNumber, uint16_t minPulse,
	   uint16_t maxPulse, uint16_t angle )
	   {
	   value16_t minPulse_; minPulse_.int16_ = minPulse;
	   value16_t maxPulse_; maxPulse_.int16_ = maxPulse;
	   value16_t angle_;    angle_.int16_    = angle;

	   writeByte( START_SYSEX );
	   writeByte( SERVO_CONFIG );
	   writeByte( pinNumber );
	   writeByte( minPulse_.int8_[0] );
	   writeByte( minPulse_.int8_[1] );
	   writeByte( maxPulse_.int8_[0] );
	   writeByte( maxPulse_.int8_[1] );
	   writeByte( angle_.int8_[0] );
	   writeByte( angle_.int8_[1] );
	   writeByte( END_SYSEX );
	   }
	   */

	void ArduinoFirmata::onProtocol( uint8_t major, uint8_t minor )
	{
		fprintf( stdout, "Protocol version : %d.%d\n", major, minor );
	}

	void ArduinoFirmata::onFirmware( std::string name, uint8_t major, uint8_t minor )
	{
		std::cout << "Firmware name : "
			<< name << std::endl;
		fprintf( stdout, "Firmware version : %d.%d\n",
				major, minor );
	}

	void ArduinoFirmata::onMessage( std::string message )
	{
		std::cout << "Message : " << message << std::endl;
	}

	void ArduinoFirmata::onAnalog( uint8_t pin, uint16_t value )
	{
		fprintf( stdout, "Analog pin[%d] = %f V\n",
				pin,
				( (float) value ) / 416.66 );
	}

	void ArduinoFirmata::processMessage( std::string& message )
	{
		unsigned int i = 0;

		while ( i < message.size() )
		{
			uint8_t begin = (uint8_t) message.at(i);

			// Protocol Version
			if ( begin == PROTOCOL_VERSION )
			{
				if ( i + 2 < message.size() )
				{
					onProtocol( (int) message.at( i + 1 ), (int) message.at( i + 2 ) );
					message.erase( i, 3 );
					continue;
				}
				else
				{
					return;
				}
			}

			// System information
			if ( begin == START_SYSEX )
			{
				unsigned int j = MAX( i, message.size() - 1 );

				for ( ; j < message.size(); ++j )
				{
					if ( (uint8_t) message.at(j) == END_SYSEX ) break;
				}

				if ( i + 1 < message.size()
						&& ( j < message.size() )
						&& ( i + 2 < message.size() ) )
					begin = (uint8_t) message.at( i + 1 );
				else
					return;

				// FIRMWARE ID
				if ( ( begin == FIRMWARE_NAME_VS )
						&& ( j - i > 4 ) )
				{
					onFirmware( message.substr( i + 4, j - i - 4 ),
							(uint8_t) message.at( i + 2 ),
							(uint8_t) message.at( i + 3 ) );
				}

				// MESSAGE
				if ( begin == STRING_MESSAGE )
				{
					onMessage( message.substr( i + 2, j - i - 3 ) );
				}

				message.erase( i, j - i + 1 );
				continue;
			}

			// Analog pin value
			if ( ANALOG_MESSAGE <= begin && begin < ANALOG_MESSAGE + 16 )
			{
				if ( i + 2 < message.size() )
				{
					value16_t value;
					value.char_[0] = message.at( i + 1 );
					value.char_[1] = message.at( i + 2 );

					onAnalog( begin % 16, value.int16_ );

					message.erase( i, 3 );
					continue;
				}
				else
				{
					return;
				}
			}

			fprintf( stdout, "0x%x\n", (unsigned char) message.at( i ) );
			fflush( stdout );
			++i;
		}

		message.clear();
	}

}

